package com.rockwell.robot2015.model.states.running.sequence;

import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.detectors.TreasureDetector;
import com.rockwell.robot2015.model.states.IState;
import com.rockwell.robot2015.model.states.IStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;

public class ApproachingTreasureSequence extends RobotRunningSequence {
	
	/**
	 * The distance where we can safely pick the treasure
	 */
	private final static double PICK_DISTANCE = 29; // %
	
	/**
	 * The distance where we should stop jogging and start indexing
	 */
	private final static double SMALL_INDEX_DISTANCE = 35; // %

	/**
	 * The distance where we should stop jogging and start indexing
	 */
	private final static double INDEX_DISTANCE = 45; // %

	/**
	 * small indexing distance
	 */
	public static int TINY_MOVE_INDEX = 1; // cm
	
	/**
	 * small indexing distance
	 */
	public static int SMALL_MOVE_INDEX = 2; // cm
	
	/**
	 * Big indexing distance
	 */
	public static int BIG_MOVE_INDEX = 10; // cm

	/**
	 * Alignment threshold beyond which we should adjust the orientation of the robot.
	 */
	private final static int ALIGNMENT_THRESHOLD = 5;	// distance from center
	
	
	enum State implements IState {
		Start {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void move(IStateMachine machine) {
				machine.robot().log("Start approaching treasure", 2);
				
				machine.changeState(AdjustOrientation);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		AdjustOrientation {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("AdjustOritentation", 4);
			}

			@Override
			public void move(IStateMachine machine) {
				switch (RobotRunningSequence.getTreasureAlignment(machine.robot(), ALIGNMENT_THRESHOLD)) {
				case Lost:
					machine.changeState(TreasureLost);
					break;
					
				case NotAligned:
					machine.changeState(TreasureNotAligned);
					break;
					
				case Aligned:
					machine.changeState(Move);
					break;
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		TreasureNotAligned {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Treasure not aligned", 4);
				
				//machine.robot().sendCommand(RobotAction.Stop);
				//machine.robot().sleep(Robot.MOTION_SETTLING_TIME);
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				// Stop, turn left/right to align treasure to center of screen,
				// then move straight
				IDetector detector = machine.robot().getTreasureDetector();
				
				if (detector.isPresent()) {
					
					int d = RobotRunningSequence.getOrientation(machine.robot(), detector);
					
					String log = String.format("Turning degree: %d", d);
					robot.log(log, 4);

					if (Math.abs(d) > ALIGNMENT_THRESHOLD) {
						robot.sendCommand(RobotAction.TurnDegree, d);
						robot.sleep(Robot.MOTION_SETTLING_TIME);
					} 
					else {
						machine.changeState(AdjustOrientation);
					}
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub

			}

		},
		Move {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("ApproachingTreasure - Move", 3);
			}

			@Override
			public void move(IStateMachine machine) {
				/* if the treasure if big enough in the screen, change to TreasureReached state
				 * otherwise send treasure angle to the controller.
				 */
				Robot robot = machine.robot();
				TreasureDetector detector = (TreasureDetector)robot.getTreasureDetector();

				if (detector.isPresent()) {
					int distance = (int)detector.getDistanceInPercentage();
					
					robot.log(String.format("Treasure distance in percentag: %d", distance), 3);
					
					if (distance <= PICK_DISTANCE) {
						// it is close enough to the treasure.
						machine.changeState(TreasureReached);
					}
					else {
						int step;
						
						if (distance > INDEX_DISTANCE) {
							// it is still far away, move a big step.
							step = BIG_MOVE_INDEX;
							robot.sendCommand(RobotAction.MoveWithSensor, step);
						}
						else if (distance > SMALL_INDEX_DISTANCE){
							// It is close but not close enough yet.
							step = SMALL_MOVE_INDEX;
							robot.sendCommand(RobotAction.MoveWithoutSensor, step);
						}
						else {
							// really close
							step = TINY_MOVE_INDEX;
							robot.sendCommand(RobotAction.MoveWithoutSensor, step);
						}
						
						robot.log(String.format("Move %d", step), 4);
						
						robot.sleep(Robot.MOTION_SETTLING_TIME);
						
						machine.changeState(AdjustOrientation);
					}
				}
				else {
					machine.changeState(TreasureReached);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		TreasureReached {

			@Override
			public void enter(IStateMachine machine) {
				// Send STOP command
				Robot robot = machine.robot();
				
				robot.log("Treasure reached", 2);
				
				robot.sendCommand(RobotAction.Stop);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
			}

			@Override
			public void move(IStateMachine machine) {
			}

			@Override
			public void exit(IStateMachine machine) {
			}

		},
		TreasureLost {

			@Override
			public void enter(IStateMachine machine) {
				Robot robot = machine.robot();

				robot.log("Treasure lost", 2);

				// Send STOP command
				robot.sendCommand(RobotAction.Stop);
				robot.sleep(Robot.MOTION_SETTLING_TIME);
			}

			@Override
			public void move(IStateMachine machine) {
			}

			@Override
			public void exit(IStateMachine machine) {
			}

		};
		
		static ApproachingTreasureSequence m_sequence;

		public static void setSequence(ApproachingTreasureSequence sequence) {
			m_sequence = sequence;
		}
		
	}

	public ApproachingTreasureSequence(RobotRunningStateMachine running) {
		super(running, State.Start);
		State.setSequence(this);
		
		// Make sure the treasure is aligned first.
		m_currState = State.Start;
	}

	@Override
	public void start() {
		Robot robot = m_running.robot();
		//robot.log("Approaching treasure", 1);
		
		m_currState = State.Start;
		m_nextState = null;
		
		robot.enableTreasureDetector(true);
		//robot.enableObstacleDetector(true);
		robot.enableMaze1Detector(true);
		robot.enableMaze2Detector(true);
		robot.enableMaze3Detector(true);
		robot.enableMazeEntranceGuideDetector(true);
		robot.enableDropAreaDetector(false);
		robot.enableOuterWallDetector(true);
	}

	@Override
	public boolean succeeded() {
		return m_currState.equals(State.TreasureReached);
	}

	@Override
	public boolean failed() {
		return m_currState.equals(State.TreasureLost);
	}

}
